#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <string.h>
#include <stdlib.h>
#include <stdint.h>

#include "rxtx.h"
#include "conf.h"

extern cnt_t cnt;
extern conf_t conf;

static struct {
	uint8_t buf[DSM2_RX_BUFFER_SIZE];
	uint8_t idx;
	uint16_t raw[DSM2_CHANNELS];
	volatile uint16_t channels[DSM2_CHANNELS];
} rx;

static struct {
	uint8_t buf[TX_BUFFER_SIZE];
	uint8_t head;
	uint8_t tail;
	uint8_t sync:1;
} tx;

void rxtx_init(void){
	rx.idx = 0;
	memset(rx.buf, 0, DSM2_RX_BUFFER_SIZE);

	tx.tail = 0;
	tx.head = 0;
	tx.sync = false;
	memset(tx.buf, 0, TX_BUFFER_SIZE);

	UCSR0A = (1 << U2X0);
	UCSR0B|= ((1 << RXEN0) | (1 << TXEN0) | (1 << RXCIE0));
	UCSR0C = (3 << UCSZ00);
	UBRR0H = ((F_CPU / 8 / DSM2_RX_BAUD) - 1) >> 8;
	UBRR0L = ((F_CPU / 8 / DSM2_RX_BAUD) - 1);
	UCSR0B|= (1 << UDRIE0);
}

void rxtx_sync_mode(bool mode){
	tx.sync = mode;
}

ISR(USART_RX_vect){
	if(cnt.rx >= DSM2_RX_DETECT_CNT)
		rx.idx = 0;

	rx.buf[rx.idx++] = UDR0;
	if(rx.idx >= DSM2_RX_BUFFER_SIZE){
		rx.idx = 0;
		dsm2_rx_channels();
	}

	cnt.rx = 0;
}

ISR(USART_UDRE_vect){
	if(!tx.sync && tx.tail != tx.head){
		uint8_t c = tx.buf[tx.tail++];
		if(tx.tail >= TX_BUFFER_SIZE)
			tx.tail = 0;
		UDR0 = c;
	}else
		UCSR0B&= ~(1 << UDRIE0);
}

void tx_push_char(char c){
	if(tx.sync){
		while(!(UCSR0A & (1<<UDRE0)));
		UDR0 = c;
		return;
	}

	tx.buf[tx.head++] = c;
	if(tx.head >= TX_BUFFER_SIZE)
		tx.head = 0;

	if(tx.head == tx.tail){
		tx.tail++;
		if(tx.tail >= TX_BUFFER_SIZE)
			tx.tail = 0;
	}
}

void tx_push_str(const char *s){
	while(*s)
		tx_push_char(*s++);
}

void tx_push_str_P(const char PROGMEM *s){
	char b;
	while(s && (b = pgm_read_byte(s++)))
		tx_push_char(b);
}

void tx_push_digit(uint32_t d){
	uint8_t i = 0;
	static char s[17];

	do{
		s[i++] = d % 10 + '0';
		d-= d % 10;
	}while((d/= 10) > 0);

	while(i)
		tx_push_char(s[--i]);
}

void dsm2_rx_channels(void){
	uint8_t i, c, bh, bl;
	uint16_t raw;

	for(i = 2, c = 0; i < DSM2_RX_BUFFER_SIZE && c < DSM2_CHANNELS; i+= 2, c++){
		bh = rx.buf[i];
		bl = rx.buf[i+1];

		if(c != ((bh >> 2) & 0x0f)){
#ifdef DEBUG
			tx_push_str("[RAW] ch_num\r\n");
			TX_SEND;
#endif
			return;
		}

		raw = ((uint16_t)(bh & 0x03) << 8) + bl;
		if(raw < 32 || raw > 1024){
#ifdef DEBUG
			tx_push_str("[RAW] val\r\n");
			TX_SEND;
#endif
			return;
		}

		rx.raw[c] = raw;
	}

	for(c = 0; c < DSM2_CHANNELS; c++){
		if(abs((int16_t)rx.raw[c] - (int16_t)rx.channels[c]) > 3){
			cnt.aft = 0;
			rx.channels[c] = rx.raw[c];
		}
	}
}

bool ch_is_min(uint8_t i){
	uint16_t v = rx.channels[i];
	return v && v < CH_MIN + CH_DB;
}

bool ch_is_mid(uint8_t i){
	uint16_t v = rx.channels[i];
	return v < CH_MID + CH_DB && v > CH_MID - CH_DB;
}

bool ch_is_max(uint8_t i){
	uint16_t v = rx.channels[i];
	return v && v > CH_MAX - CH_DB;
}

void dump_channels(void){
#ifdef DEBUG
	for(uint8_t i = 0; i < DSM2_CHANNELS; i++){
		tx_push_digit(i+1);
		tx_push_char(':');
		tx_push_digit(rx.channels[i]);
		tx_push_char(' ');
	}

	tx_push_char('\r');
	TX_SEND
#endif
}
